a8irPort = 1
cpPort = 2
clPort = 3
motorPortL = 1
motorPortR = 3
enemyGoal = 350
reverse = 1

nxt.I2Cversion    = string.char( 0x10, 0x00 )
nxt.I2Cproduct    = string.char( 0x10, 0x08 )
nxt.I2Ctype       = string.char( 0x10, 0x10 )
nxt.I2Ccontinuous = string.char( 0x10, 0x41, 0x02 )
nxt.I2Cdata       = string.char( 0x10, 0x42 )

function waitI2C( port )
  while( 0 ~= nxt.I2CGetStatus( port ) ) do
  end
end

function setupI2C(port)78

  nxt.InputSetType(port,2)
  nxt.InputSetDir(port,1,1)
  nxt.InputSetState(port,1,1)
  nxt.I2CInitPins(port)
end

function IRHeading(port)
	nxt.I2CSendData(port, string.char( 0x10, 0x42 ), 8)
	waitI2C(port)
	return string.byte(nxt.I2CRecvData(port))
end
function compHeading(port)
	nxt.I2CSendData(port,  string.char( 0x02, 0x42 ), 4)
	waitI2C(port)
	data = nxt.I2CRecvData(port, 4)
	b1, b2, b3, b4 = string.byte(data, 1, 8) 
	if b4 == 1 then
		truevalue = b3 + 255
	else
		truevalue = b3
	end
	return truevalue
end

function degAdd(a, b)
	c = a + b
	if c >= 360 then
		c = c - 360
	end
	if c < 0 then
		c = c + 360
	end
	return c
end
function degDiff(a, b)
	c = a - b
	if c < 0 then
		c = c  * -1
	end
	if c > 180 then
		c = 360 - c
	end
	return c
end
function sleep(ms)
	endtime = nxt.TimerRead() + ms
	while nxt.TimerRead() < endtime do
	end
end
function whichSide(me, it)
    if me < it then
        if (it - me) < 180 then
            return 1
        else
            return 0
        end
    else
        if (me - it) < 180 then
            return 0
        else
            return 1
        end
    end
end

-- Driving functions
function steerForward(speed)
	speed = speed*reverse
	nxt.OutputSetSpeed(motorPortL, 32, speed)
	nxt.OutputSetSpeed(motorPortR, 32, speed)
end
function steerLeft(speed)
	speed = speed*reverse
	nxt.OutputSetSpeed(motorPortL, 32, speed)
	nxt.OutputSetSpeed(motorPortR, 32, -speed)
end
function steerRight(speed)
	speed = speed*reverse
	nxt.OutputSetSpeed(motorPortL, 32, -speed)
	nxt.OutputSetSpeed(motorPortR, 32, speed)
end
function allStop()
	nxt.OutputSetSpeed(motorPortL, 0, 0)
	nxt.OutputSetSpeed(motorPortR, 0, 0)
end

setupI2C(irPort)
setupI2C(cpPort)
attack = true
run = false
while true do
	heading = compHeading(cpPort)
	ball5 = IRHeading(irPort)
	ball = (ball5 - 5)*30
	ball = degAdd(ball, 0)
	ballH = degAdd(ball, heading)
	if nxt.ButtonRead() == 8 then
		enemyGoal = heading
	end
	if degDiff(ballH, enemyGoal) < 80 then
		spd = 80
		if degDiff(0, ball) < 50 then
			spd = 70
		end
		if ball ~= 0 then
			if whichSide(ball, 0) == 1 then
				steerRight(spd)
			else
				steerLeft(spd)
			end
		else
			steerForward(80)
		end
	else
		allStop()
	end
	sleep(100)
	if nxt.ButtonRead() ~= 0 then
		allStop()
		break
	end
end
